#!/usr/bin/env python
import rospy
from std_msgs.msg import Bool
from slam_toolbox_msgs.srv import DeserializePoseGraphRequest, DeserializePoseGraph, SerializePoseGraphRequest, SerializePoseGraph


def map_save_callback(msg):
    print("save map")
    req = SerializePoseGraphRequest()
    req.filename = "/home/nucros/orimap"
    rospy.wait_for_service("/slam_toolbox/serialize_map")
    try: 
        srv = rospy.ServiceProxy("/slam_toolbox/serialize_map", SerializePoseGraph)
        res = srv.call(req)
    except rospy.ServiceException:
        print("Service call failed")     

    print("map saved :res")

def remap(msg):
    print("remap")
    rospy.wait_for_service("/slam_toolbox/deserialize_map")
    try: 
        srv = rospy.ServiceProxy("/slam_toolbox/deserialize_map", DeserializePoseGraph)
        req = DeserializePoseGraphRequest()
        req.filename = "/home/nucros/orimap"
        req.initial_pose.x = 0
        req.match_type = 1
        res = srv.call(req)
    except rospy.ServiceException:
        print("Service call failed")     

def main(args=None):
    rospy.init_node('inremental_mapping')
    map_save_sub = rospy.Subscriber("/save_sl_map", Bool, map_save_callback)
    remap_sub = rospy.Subscriber("/remap", Bool, remap)
    rospy.spin()


if __name__ == '__main__':
    main()
